#ifndef ASTARSOLVER_H
#define ASTARSOLVER_H

#include <QDebug>
#include <QVector>
#include <QHash>
#include <queue>
#include <QWaitCondition>
#include <QMutex>
#include <QObject>
#include <QRunnable>

#define Loc_with_cost QPair<float, Location>
#define MAP QVector<QVector<int>>

struct Location
{
    int x, y;
};
inline bool operator==(Location a, Location b)
{
    return (a.x == b.x) && (a.y == b.y);
}
inline bool operator<(Location a, Location b)
{
    return (a.x < b.x) || (a.y < b.y);
}
QDebug& operator<<(QDebug& out, const Location& loc);

enum
{
    UNDEFINED = -1,
    ROAD = 0,   // 道路
    WALL = 1    // 墙
};

inline uint qHash(const Location& key, uint seed = 0)
{
    return (key.x ^ (key.y << 16));
}


class AStarSolver : public QObject
{
    Q_OBJECT
public:
    AStarSolver();

    // 地图大小
    int width()
    {
        if (map.empty())
            return 0;
        else
            return map[0].size();
    }
    int height() { return map.size(); }

    int mapAt(int x, int y)const;
    void readMap(QString map_path);

    void search(Location start, Location goal);
    void search(int startx, int starty, int goalx, int goaly);

    void stop();

    enum HMethod
    {              // 启发函数
        Manhattan, // 曼哈顿距离
        Euclidean  // 欧氏距离
    };
    void setHMethod(HMethod h) { m_hMethod = h; }

private:
    // A*搜索主算法
    void search(Location start, Location goal,
                QHash<Location, Location>& came_from,
                QHash<Location, float>& cost_so_far);
    // 启发函数计算
    inline float heuristic(Location a, Location b);
    QVector<Location> getNeighbors(Location loc);
    QVector<Location> DIRS = {  // 八邻接
        Location{1, 0}, Location{-1, 0}, Location{0, -1},  Location{0, 1},
        Location{1, 1}, Location{-1, 1}, Location{-1, -1}, Location{1, -1},
    };
    QVector<Location> DIRS4 = { // 四邻接
        Location{1, 0}, Location{-1, 0}, Location{0, -1},  Location{0, 1},
    };
    float cost(Location a, Location b);
    bool in_bounds(Location);
    bool passable(Location);
    void initMap();
    void printMap();
    QVector<Location> reconstruct_path(Location start,Location goal);

    bool m_stop=false;
signals:
    void step();

public:                        // 定义为public成员，便于MainWindow读取
    MAP map; // 地图

    HMethod m_hMethod; // 选用哪个启发函数

    QVector<Loc_with_cost> frontier;    // 搜索范围
    QVector<Location> neighbors;        // 当前邻居

    QHash<Location, Location> came_from; // 记录路径信息
    Location current;   // 当前位置
    Location current_best;              // 下一步最佳点
    QHash<Location, float> cost_so_far; // 当前路径代价
    QHash<Location, float> cost_heuristic; // 启发函数估计代价
    QHash<Location, float> cost_total; // 总估计代价

    QVector<Location> path; // 搜索到的路径

    enum Mode{
        AUTO,
        MANUAL
    }mode;
    enum State{
        FREE,
        RUNNING,
        WAITING,
        EXECUTE,
        SLEEPING
    }state;
};

#endif // ASTARSOLVER_H
